#include "pose.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "move.h"
#include "uarttouart.h"
#include "RISCV_Typedefs.h" // 添加数据类型定义
#include "board_delay.h"
#include "mpu6050.h"

#define DELAY_MS(ms) simple_delay_ms(ms)

// 解析二维码误差数据帧
QR_ErrData_t Pose_ParseQRData(const char* uart_str)
{
    QR_ErrData_t result = {0};
    result.valid = false;
    if (!uart_str) return result;
    // 检查帧头
    if (strncmp(uart_str, "$ERR,", 5) != 0) return result;
    // 解析数据
    const char* p = uart_str + 5;
    char* endptr;
    // 1. 解析ID
    result.id = (int)strtol(p, &endptr, 10);
    if (*endptr != ',') return result;
    p = endptr + 1;
    // 2. 解析err_x
    result.err_x = strtof(p, &endptr);
    if (*endptr != ',') return result;
    p = endptr + 1;
    // 3. 解析err_y
    result.err_y = strtof(p, &endptr);
    if (*endptr != ',') return result;
    p = endptr + 1;
    // 4. 解析err_theta
    result.err_theta = strtof(p, &endptr);
    // 允许结尾有换行或结束符
    result.valid = 1;
    return result;
}

// 根据误差大小计算调整速度
float32_t CalculateAdjustSpeed(float error, float max_error, float min_speed, float max_speed) {
    float32_t error_ratio = fabsf(error) / max_error;
    if (error_ratio > 1.0f) error_ratio = 1.0f;
    return min_speed + (max_speed - min_speed) * error_ratio;
}

// 根据误差大小计算调整时间
uint32_t CalculateAdjustTime(float error, float max_error, uint32_t min_time, uint32_t max_time) {
    float32_t error_ratio = fabsf(error) / max_error;
    if (error_ratio > 1.0f) error_ratio = 1.0f;
    return min_time + (uint32_t)((max_time - min_time) * error_ratio);
}

// 改进的姿态调整主函数（循环调整直到满足精度要求）
void Pose_AdjustByQR(const QR_ErrData_t* err, float xy_tol, float theta_tol)
{
    printf("调试：id=%d, x=%.2f, y=%.2f, theta=%.2f, valid=%d\r\n", err->id, err->err_x, err->err_y, err->err_theta, err->valid);
    if (!err || !err->valid) return;

    float abs_theta = err->err_theta > 0 ? err->err_theta : -err->err_theta;
    float abs_x = err->err_x > 0 ? err->err_x : -err->err_x;
    float abs_y = err->err_y > 0 ? err->err_y : -err->err_y;

    // 定义调整参数
    const float32_t min_speed = 0.1f;
    const float32_t max_speed = 0.3f;
    const uint32_t min_time = 1000;
    const uint32_t max_time = 4000;
    const float max_angle_error = 30.0f;
    const float max_pos_error = 20.0f;

    float32_t adjust_speed = 0.0f;
    uint32_t adjust_time = 0;

    if (abs_theta >= theta_tol) {
        printf("进入角度调整分支\r\n");
        adjust_speed = CalculateAdjustSpeed(err->err_theta, max_angle_error, min_speed, max_speed);
        adjust_time = CalculateAdjustTime(err->err_theta, max_angle_error, min_time, max_time);
        if (err->err_theta > 0) {
            printf("角度调整：右转 %.2f 速度，%d ms\r\n", adjust_speed, adjust_time);
            Move_RotateRight(adjust_speed);
        } else {
            printf("角度调整：左转 %.2f 速度，%d ms\r\n", adjust_speed, adjust_time);
            Move_RotateLeft(adjust_speed);
        }
        DCMotor_UpdateAllMotors();
        DELAY_MS(adjust_time);
        Move_Stop();
        DCMotor_UpdateAllMotors();
        return;
    } else if (abs_x >= xy_tol) {
        printf("进入左右调整分支\r\n");
        adjust_speed = CalculateAdjustSpeed(err->err_x, max_pos_error, min_speed, max_speed);
        adjust_time = CalculateAdjustTime(err->err_x, max_pos_error, min_time, max_time);
        if (err->err_x > 0) {
            printf("左右调整：右移 %.2f 速度，%d ms\r\n", adjust_speed, adjust_time);
            Move_Right(adjust_speed);
        } else {
            printf("左右调整：左移 %.2f 速度，%d ms\r\n", adjust_speed, adjust_time);
            Move_Left(adjust_speed);
        }
        DCMotor_UpdateAllMotors();
        DELAY_MS(adjust_time);
        Move_Stop();
        DCMotor_UpdateAllMotors();
        return;
    } else if (abs_y >= xy_tol) {
        printf("进入前后调整分支\r\n");
        adjust_speed = CalculateAdjustSpeed(err->err_y, max_pos_error, min_speed, max_speed);
        adjust_time = CalculateAdjustTime(err->err_y, max_pos_error, min_time, max_time);
        if (err->err_y > 0) {
            printf("纵向调整：后退 %.2f 速度，%d ms\r\n", adjust_speed, adjust_time);
            Move_Backward(adjust_speed);
        } else {
            printf("纵向调整：前进 %.2f 速度，%d ms\r\n", adjust_speed, adjust_time);
            Move_Forward(adjust_speed);
        }
        DCMotor_UpdateAllMotors();
        DELAY_MS(adjust_time);
        Move_Stop();
        DCMotor_UpdateAllMotors();
        return;
    } else {
        printf("姿态调整完成，误差在容忍范围内\r\n");
        Move_Stop();
        DCMotor_UpdateAllMotors();
        DELAY_MS(500);
    }
}

// 改进的姿态调整函数：循环调整直到满足精度要求
bool Pose_AdjustByK230UART5(float xy_tol, float theta_tol, uint32_t timeout_ms)
{
    const int max_adjust_attempts = 10;  // 最大调整次数
    const uint32_t verify_delay = 1000;  // 验证延时(ms)
    
    char err_buffer[128];
    QR_ErrData_t qr_err;
    
    printf("开始姿态调整，目标精度：位置%.1fcm，角度%.1f度\r\n", xy_tol, theta_tol);
    
    for (int attempt = 1; attempt <= max_adjust_attempts; attempt++) {
        printf("姿态调整第%d次尝试...\r\n", attempt);
        
        // 1. 轮询接收ERR数据
        int len = UART5_ReceiveERRData(err_buffer, sizeof(err_buffer), timeout_ms);
        if (len <= 0) {
            printf("第%d次尝试：无法接收K230数据\r\n", attempt);
            continue;
        }
        
        // 2. 解析数据
        qr_err = Pose_ParseQRData(err_buffer);
        if (!qr_err.valid) {
            printf("第%d次尝试：数据解析失败\r\n", attempt);
            continue;
        }
        
        printf("当前误差：ID=%d, X=%.1f, Y=%.1f, θ=%.1f\r\n", 
               qr_err.id, qr_err.err_x, qr_err.err_y, qr_err.err_theta);
        
        // 3. 检查是否已经满足精度要求
        if (fabsf(qr_err.err_x) < xy_tol && 
            fabsf(qr_err.err_y) < xy_tol && 
            fabsf(qr_err.err_theta) < theta_tol) {
            printf("姿态调整成功！误差满足要求\r\n");
            return true;
        }
        
        // 4. 执行姿态调整
        Pose_AdjustByQR(&qr_err, xy_tol, theta_tol);
        
        // 5. 等待调整效果稳定
        DELAY_MS(verify_delay);
    }
    
    printf("姿态调整失败：达到最大尝试次数%d次\r\n", max_adjust_attempts);
    return false;
}

// 新增：快速姿态调整函数（用于移动前的粗略调整）
bool Pose_QuickAdjust(float xy_tol, float theta_tol, uint32_t timeout_ms)
{
    const int max_quick_attempts = 3;  // 快速调整最大次数
    
    char err_buffer[128];
    QR_ErrData_t qr_err;
    
    printf("开始快速姿态调整...\r\n");
    
    for (int attempt = 1; attempt <= max_quick_attempts; attempt++) {
        // 接收数据
        int len = UART5_ReceiveERRData(err_buffer, sizeof(err_buffer), timeout_ms);
        if (len <= 0) continue;
        
        // 解析数据
        qr_err = Pose_ParseQRData(err_buffer);
        if (!qr_err.valid) continue;
        
        // 只进行角度调整（最关键的）
        if (fabsf(qr_err.err_theta) >= theta_tol) {
            float32_t adjust_speed = 0.08f;
            uint32_t adjust_time = 1500;
            
            if (qr_err.err_theta > 0) {
                Move_RotateRight(adjust_speed);
            } else {
                Move_RotateLeft(adjust_speed);
            }
            DCMotor_UpdateAllMotors();
            DELAY_MS(adjust_time);
            Move_Stop();
            DCMotor_UpdateAllMotors();
        } else {
            printf("快速姿态调整完成\r\n");
            return true;
        }
        
        DELAY_MS(500);
    }
    
    printf("快速姿态调整超时\r\n");
    return false;
} 

// 新增：基于MPU6050的姿态辅助矫正
void Pose_AdjustByMPU6050(float pitch_target, float roll_target, float pitch_tol, float roll_tol)
{
    MPU6050_Attitude_t att;
    if (MPU6050_GetAttitude(&att) != 0) {
        printf("MPU6050 姿态获取失败！\r\n");
        return;
    }
    printf("当前姿态: pitch=%.2f, roll=%.2f\r\n", att.pitch, att.roll);
    float pitch_err = att.pitch - pitch_target;
    float roll_err = att.roll - roll_target;
    if (fabsf(pitch_err) > pitch_tol) {
        if (pitch_err > 0) {
            printf("前倾过大，后退矫正\r\n");
            Move_Backward(0.1f);
        } else {
            printf("后仰过大，前进矫正\r\n");
            Move_Forward(0.1f);
        }
        DCMotor_UpdateAllMotors();
        DELAY_MS(500);
        Move_Stop();
        DCMotor_UpdateAllMotors();
    }
    if (fabsf(roll_err) > roll_tol) {
        if (roll_err > 0) {
            printf("右倾过大，左移矫正\r\n");
            Move_Left(0.1f);
        } else {
            printf("左倾过大，右移矫正\r\n");
            Move_Right(0.1f);
        }
        DCMotor_UpdateAllMotors();
        DELAY_MS(500);
        Move_Stop();
        DCMotor_UpdateAllMotors();
    }
} 